#ifndef ESKF_H
#define ESKF_H

#include <Eigen/Core>
#include "../ImuTypes.h"
#include "../PoseFrame.h"
#include "State.h"
#include "WheelOdom.h"

namespace msf
{

class ESKF {
 public:
      StatePtr state_ptr_;

      //ESKF() = delete; //表示这个类的默认构造函数被删除，不能创建这个类的对象

      ESKF(const ESKF &) = delete; //表示这个类的拷贝构造函数被删除，不能拷贝这个类的对象

      // explicit ESKF(const PoseFrame &poseframe) // 必须显式地调用这个构造函数
      // {
      //       state_ptr_ = std::make_shared<State>();
      // }

      explicit ESKF(Sophus::SE3<float> &Tbc) // 必须显式地调用这个构造函数，防止隐式地转换类型
      {
            state_ptr_ = std::make_shared<State>(Tbc);
      }

      /**
      * @brief predict procedure
      * @param last_imu
      * @param curr_imu
      * @param last_poseframe
      * @param init
      */
      void predict(const IMU::Point last_imu, const IMU::Point curr_imu, PoseFrame last_poseframe, bool init) {
            State last_state = *state_ptr_;
            IMU::Calib calib = last_poseframe.mImuCalib;

            // 如果imu已经初始化过了
            if(init)
            {
                  // last_state中的位姿和bias更新为mLastPoseFrame中的
                  last_state.p_wb_ = last_poseframe.GetImuPosition().cast<double>(); // Eigen::Vector3d // Eigen::Matrix<float,3,1>
                  last_state.v_wb_ = last_poseframe.GetVelocity().cast<double>(); // Eigen::Vector3d // Eigen::Vector3f
                  last_state.Rwb_ = last_poseframe.GetImuRotation().cast<double>(); // Eigen::Matrix3d // Eigen::Matrix<float,3,3>
                  last_state.acc_bias = Eigen::Vector3d(last_poseframe.mImuBias.bax, last_poseframe.mImuBias.bay, last_poseframe.mImuBias.baz); // Eigen::Vector3d
                  last_state.gyr_bias = Eigen::Vector3d(last_poseframe.mImuBias.bwx, last_poseframe.mImuBias.bwy, last_poseframe.mImuBias.bwz); // Eigen::Vector3d
            }

            state_ptr_->timestamp = curr_imu.t; 
            state_ptr_->delta_t = state_ptr_->timestamp - last_state.timestamp;

            IMU::propagate_state(last_imu, curr_imu, last_state, *state_ptr_, calib);
            IMU::propagate_state_cov(last_imu, curr_imu, last_state, *state_ptr_, calib);

            // 更新原始imu的角速度数据供correct使用
            state_ptr_->curr_w = curr_imu.w; // Eigen::Vector3f
      }

      /**
      * @brief correct procedure
      * @param measurement
      */
      void correct(const OdomData measurement)
      {
            // 轮速计速度转换到IMU坐标系下
            // const auto &p_G_Odom = WheelOdom::g2l(measurement);
            // measurement->linear_vel; // Eigen::Vector3d
            // measurement->angular_vel; // Eigen::Vector3d
            // odom和imu坐标变换关系: state_ptr_->Tbo; // Sophus::SE3<float>

            Eigen::Vector3d linear_vel_m = state_ptr_->Tbo.so3().matrix().cast<double>() * measurement.linear_vel;
            Eigen::Vector3d angular_vel_m = state_ptr_->Tbo.so3().matrix().cast<double>() * measurement.angular_vel;

            // 轮速计的线速度转换到世界坐标系下
            // imu和world坐标变换关系: state_ptr_->Rwb_ // Eigen::Matrix3d
            linear_vel_m = state_ptr_->Rwb_ * linear_vel_m;

            // 获取state_ptr_中预测得到的线速度v和IMU原始的omega角速度
            // const Eigen::Isometry3d &Twb = state_ptr_->pose();
            // state_ptr_->v_wb_; // Eigen::Vector3d
            // state_ptr_->curr_w // Eigen::Vector3f
            Eigen::Vector3d linear_vel_p = state_ptr_->v_wb_;
            Eigen::Vector3d angular_vel_p = state_ptr_->curr_w.cast<double>();

            // 得到对误差状态的观测z
            //const auto &residual = WheelOdom::measurement_residual(Twb.matrix(), p_G_Odom); // 常量引用可以避免复制对象，节省内存和时间，同时保证不会修改引用的对象
            Eigen::Vector3d linear_vel_res = linear_vel_m - linear_vel_p;
            Eigen::Vector3d angular_vel_res = angular_vel_m - angular_vel_p;       
            Eigen::Matrix<double,15,1> residual;
            residual.setZero();
            residual.block<3,1>(3,0) = linear_vel_res;
            residual.block<3,1>(6,0) = angular_vel_res;

            // 计算H
            //const auto &H = WheelOdom::measurement_jacobian(Twb.matrix(), p_G_Odom);
            Eigen::Matrix<double, kStateDim, 15> H;
            H.setZero();
            H.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity();
            H.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity(); 

            // 计算K = P*H.t / inv(HPH.t + R)
            Eigen::Matrix<double, kStateDim, 15> K;
            const Eigen::Matrix<double,15,15> &R = measurement.cov; // dimension:15*15

            update_K(H, R, K);

            // 误差状态 K * residual
            Eigen::Matrix<double,15,1> state_update = K*residual; // 15*1

            // 更新状态
            //*state_ptr_ = *state_ptr_ + (K * residual); // 注意：state_ptr_是一个指针，而不是一个对象，无法直接使用+运算符对其进行操作。需要先解引用指针，然后再调用State类型的operator+()方法进行操作
            state_ptr_->v_wb_ += state_update.block<3,1>(3,0);
            state_ptr_->Rwb_ = State::rotation_update(state_ptr_->Rwb_, State::delta_rot_mat(state_update.block<3,1>(6,0)));
            state_ptr_->p_wb_ += state_update.block<3,1>(3,0) * state_ptr_->delta_t;

            // 更新误差状态协方差矩阵P = (I - K*H) * P = state_ptr_->cov
            update_P(H, R, K);

            // ESKF重置
            // 误差部分置0
            // 协方差部分不变
      }

      //template <class H_type, class R_type, class K_type>
      void update_K(const Eigen::Matrix<double, kStateDim, 15> &H, const Eigen::Matrix<double,15,15> &R, Eigen::Matrix<double, kStateDim, 15> &K) {
            //Problem5 K出现nan : S奇异，没有逆矩阵，所以出现了nan
            const auto &P = state_ptr_->cov;
            const Eigen::Matrix<double,15,15> &S = H * P * H.transpose() + R;

            // 判断矩阵S是否奇异
            //const auto &A = S.determinant();

            //K = P * H.transpose() * S.inverse(); // replace this line with the following

            //Eigen::HouseholderQR<Eigen::Matrix<double,15,15>> qr(S); // perform QR decomposition on S
            // 使用JacobiSVD类来求解x
            Eigen::JacobiSVD<Eigen::MatrixXd> svd(S, Eigen::ComputeThinU | Eigen::ComputeThinV); // 对A进行SVD分解
            K = P * H.transpose() * svd.solve(Eigen::Matrix<double, 15, 15>::Identity()); // solve the linear system using QR
      }

      //template <class H_type, class R_type, class K_type>
      void update_P(const Eigen::Matrix<double, kStateDim, 15> &H,
                  const Eigen::Matrix<double,15,15> &R,
                  const Eigen::Matrix<double, kStateDim, 15> &K) {
            const MatrixSD &I_KH = MatrixSD::Identity() - K * H;
            state_ptr_->cov = I_KH * state_ptr_->cov * I_KH.transpose() + K * R * K.transpose();
      }

      ~ESKF() {}

};

using ESKFPtr = std::unique_ptr<ESKF>;
} // namespace msf

#endif